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TECHNICAL PAPER 


TECHNIQUE TO ELIMINATE COMPUTATIONAL INSTABILITY IN MULTIBODY 
SIMULATIONS EMPLOYING THE LAGRANGE MULTIPLIER 


I. INTRODUCTION 


The Lagrange multiplier method has been used for many years by the author and other 
engineers to develop the equations of motion in multibody problems. An important application for this 
method is in the simulation of a space shuttle solid rocket booster (SRB) which is being decelerated 
and stabilized for water impact by a drogue parachute or three main parachutes. Reference 1 
describes one of the first digital computer programs at Marshall Space Flight Center (MSFC) that 
employed the Lagrange multiplier method to simulate SRB-parachute dynamics. 

Use of the Lagrange multiplier has an advantage over some other formulations in that it pre- 
serves the original simple form of the equations of motion for each body. However, in typical time- 
varying dynamics simulations that use the Lagrange multiplier, computational instability can occur 
because integration error causes the two (or more) attached bodies to drift apart and violate the 
constraints. The computational instability usually happens suddenly, and the computed values “blow 
up” within a few time steps. Computational instability occurred often in SRB-parachute dynamics 

work at MSFC in the 1970’s resulting in much frustration for those involved. 

* 

This paper describes a technique to eliminate computational instability caused by drifting 
apart of the attached bodies when the Lagrange multiplier is used. The equations of motion are not 
changed; instead, the programming of the equations on a digital computer is changed. A computer 
program listing is provided in the appendix to aid in the description of the programming technique. 


n. EQUATIONS OF MOTION WITH LAGRANGE MULTIPLIER 


For the sake of completeness, the multibody equations of motion employing the Lagrange 
multiplier will first be presented. A simple dynamics problem has been selected as the example so 
that the basic principles can be clearly illustrated. The example consists of two rigid bodies con- 
nected by a frictionless swivel. Each body would have six degrees of freedom (DOF) if not con- 
nected to the other body. 

The development of the equations of motion with the Lagrange multiplier begins with the 
general Newtonian equations that will be written for each body at its center of mass (CM) in a 
body-fixed frame. The equations for either body have the familiar vector form: 



Translation 


MASS*VDOT = FORCE - WxV * MASS , 


Rotation 


(1A) 


HDOT = MOMENT - WxH , ( { B) 

where V is the ve locity of the CM; W and H are the angular velocity and momentum, respectively; 
VDOT a nd // DOT are t ime derivatives taken in the body frame; and MASS is a scalar quantity. The 
FORCE and MOMENT vectors include both the external and attach point constraint components. 

The Newtonian equations for each body must be converted to matrix form by using inertia 
matrices and tilde matrices. The equations for both bodies can then be combined into the following 
matrix equation: 


[M][DQ] = [F£] + [FA] + [FW] , 


( 2 ) 


where [M] is a 12x12 quasi-diagonal matrix containing the mass and inertia matrices for both 
bodies which w ill be descr ibed in detail later; [DQ] is a 12x1 matrix containing the body-fixed com- 
ponents of the VDOT and WDOT accelerations for each body; [FE] is a 12x1 matrix containing the 
external forces and moments for each body; [FA] is a 12x1 matrix containing the attach point con- 
straint forces and moments expressed at the CM of each body; and [FW] is a 12x1 matrix equivalent 

of the (-WxV*MASS) and (-WxH) vectors for each body. The calculation of [F’W] is described in 
the computer program listing in the appendix. 

At this point, the subscripts B and C are chosen to represent the two bodies, and the follow- 
ing sequence is specified for the cells in the [M] matrix and the 12x1 matrices in equation (2): 


First cell — translation of body B 
Second cell — rotation of body B 
Third cell — translation of body C 
Fourth cell — rotation of body C 

[Af], the 12x12 quasi-diagonal matrix for masses and inertias in equation (2), thus has the form: 


[M] 


‘[MASSs] 

[IM b ] 0 

[MASSc] 

0 [IM C ] 


where [MASSb] and [MASSc] are 3x3 diagonal matrices containing the mass of each body, and 
[IMb] and [I Me] are the 3x3 inertia matrices in body-fixed frames. 


[DQ] in equation (2) is a 12x1 matrix with the following four 3x1 cells containing the body- 
fixed components of translational and rotational acceleration for both bodies: 
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[Dfl] = 


[VDOT b ] 

[WDOT b ] 

[VDOT c ] 

[WDOT c ] 


A 12x1 velocity matrix, [Q], will be defined similarly to [DQ). Its four 3x1 cells are: 


m = 


[V B ] 

[W B ] 

[V C ] 

L [Wd. 


where [V B ] and [V c ] contain the body-fixed components of the velocity of the CM for each body, and 
[Wb] and [W c ] contain the body-fixed angular velocity components for each body. 

Continuing with the Newtonian equations, equation (2) will be rearranged using the inverse 
of [M] as follows: 


l DQ} = [M]~ l [[F]+[FA ]] , (3) 

where [F] is simply the sum of [FE] and [FW]. If the attach point constraint forces and moments, 
[FA], were known, equation (3) could be used to determine the translational and rotational accel- 
erations of both bodies as is done in a typical dynamics problem. 

To calculate [FA], the Lagrange multiplier method will be used. To prepare for the incorpora- 
tion of the Lagrange multiplier, the attach point constraint equation, which states the velocities of the 
attach points of both bodies are equal, will be presented. In vector form, the attach point constraint 
equation for the chosen example is: 

V b +W b xL B = V c +W c xL c , (4) 

where the subscripts B and C represent the two bodies (as is the case throughout this paper); V and 
W are the velocity of the CM and angular velocity previously defined; and L is the attach point vector 
which defines the distance from the CM to the attach point for each body. 

Equation (4) must be converted to a matrix form that will allow the 12x1 [Q] matrix to be 

factored out. To do this, a vector, U, will be substituted for the (WxL) cross-product of each side of 
equation (4): 

Vb+Ub = Vc+Uc ■ (5) 

Equation (5) can easily be converted to a matrix equation in the C body frame by using a 
transformation matrix, [A CB ] , to transform from the B body frame to the C body frame. The resulting 
equation is: 


[ACB][V b ] + [ACB][U b ] = [V c ] + [Ud ■ 


( 6 ) 
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Returning to the U vectors in equation (5) they represent the following cross products: 

Ub — and Uc = W^xLc • ( 7 ) 

These two cross products can be rearranged by reversing their order and inserting a minus 
sign, producing: 

Ub = -TflXWfl and U c = -L c xW c . (g) 

♦ 

By using the tilde matrix for each attach point vector, L , the two parts of equation (8) can be 
converted to matrix form as follows: 

Wb\ = -[ LTbWb] and [U c ] = -[LT C )[W C ] , (9) 

where [LTb\ and [LTcl are tilde matrices for the attach point vectors. The tilde matrices and angular 
velocity matrices are all expressed in their original frames. 

By substituting both parts of equation (9) back into equation (6), the following equation is 
obtained: 

[ACB][Vb]-[ACB][LT b ][W b ] = [V C ]-[LT C ][W C ] , (10) 

where every term is a 3x1 matrix. 

After gathering all the terms in equation (10) to the left side, we have: 

[ACB][V b ]-[ACB][LTb][Wb]-[V c ] + [LTc][Wc] = 0 , (11) 

Equation (11), the constraint equation, is now in a form that will allow the 12x1 [ft] matrix to 
be factored out. After factoring out [ft], the reconfigured constraint equation is: 

[A}[Q1 = 0 , (12) 

where [A] is a 3x12 matrix that will be called the “constraint matrix.” By inspection of the terms in 
equation (11), [A] can be expressed as four 3x3 cells as follows: 

[A] = [l ACB ] \ -[. ACB][LT b ] : -[IDENT] i [ LT C ]] , 
where [IDENT] is a 3x3 identity matrix. 

Continuing with the derivation, equation (12) will be differentiated with respect to time, which 
gives the following: 


[A][Z>ft] + [AZ>Or][ft] = 0 . 

Substituting the expression for [Dft] from equation (3) into equation (13) yields: 
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[A][M]~'[[F]+[FA]]+[ADOT\[n] = 0 . 


(14) 


Equation (14) will be rearranged as follows: 

[A][MY l [FA] = -[A][Mr l [F]-[ADOT\[0] • (15) 

[FA], the attach point constraint forces and moments, must be determined to finish the 
derivation of the equations of motion. Unfortunately, no inverse exists for the 3x12 matrix term 
[A][M] _1 in equation (15), preventing a direct solution for [FA]. 

To obtain [FA], the Lagrange multiplier method which uses the following derivable relation- 
ship for the attach point constraint forces and moments will be introduced: 

[FA] = [Af[ A] , (16) 

where [A] 7 " is the transpose of the “constraint matrix,” and [A] is the 3x1 Lagrange multiplier 
matrix. 


The derivation of the Lagrange multiplier relationship in equation (16) will not be presented in 
this paper. The reader is urged to study reference 2 for an excellent explanation of the Lagrange 
multiplier and related subjects. 

To continue the calculation of [FA], note that [A] can be determined by first combining equa- 
tions (15) and (16) to eliminate [FA], which produces the following: 

[A][AT] -1 [A] r [A] = -[A][Mr l [F]-[ADOT][n] . (17) 

The matrix, [A][Af] _1 [A] r , is a 3x3 which has an inverse. Therefore, equation (17) can be 
used to solve directly for [A] as follows: 

[A] = [[A][Mr l [A] T Y l [-[A][M]- l [F}-[ADOT}m] • ( 18 ) 

To calculate [FA], the expression for [A] in equation (18) is substituted back into equation 
(16) to produce the following: 

[FA] = [A] r [[A][M]- 1 [A] 7 ']~ 1 [-[A][M]- 1 [F]-[AZ)071[^]] . (19) 

The derivation of [FA], the attach point constraint forces and moments, is now complete. 
Equation (19) can be used to calculate [FA] as a function of the known system parameters such as 
mass, geometry, external forces, and velocities. The components of [FA] are expressed at the CM of 
each body, not at the attach point. 
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in. GENERAL PROGRAMMING COMMENTS 


After [FA] has been calculated in the computer program, it is inserted in equation (3) to cal- 
culate [DQ] which contains the 12 acceleration components. For convenience, equation (3) is again 
presented: 


[DQ] = [M]~ l [[F]+[FA]] . (3) 

In all previous simulations which used the Lagrange multiplier and which are known to the 
author, the 12 accelerations in equation (3) were integrated to obtain velocity and angular velocity 
components. After the integration had been performed over a period of time, integration error caused 
the two bodies to drift apart and violate the constraints, which eventually led to computational 
instability. 


IV. TECHNIQUE TO ELIMINATE COMPUTATIONAL INSTABILITY 


The technique that eliminates computational instability caused by drifting of the attached 
bodies will now be presented. Instead of integrating the 12 acceleration coordinates in equation (3), 
the 2 bodies in the chosen example will be examined to determine the independent coordinates. One 
can see that there are only nine independent coordinates in the example: three rotational coordinates 
for each body (total of six coordinates) and three translational coordinates for one of the bodies. 
Body B will be chosen as the body whose translational coordinates are independent, meaning that 
the translational coordinates of body C are dependent upon the other nine coordinates. Only the nine 
independent acceleration coordinates will be integrated to get the three translational velocity compo- 
nents of body B and the six angular velocity components for both bodies. The three translational 
velocity components of body C must somehow be calculated. To do this, one needs only to take 
equation (11), the constraint equation, and solve it for the velocity of body C: 

[V c ] = [ ACB][V b ]-[ACB][LT b ][W b ] + [LT c ][Wc ] , (20) 

where all matrices have been previously defined. By using equation (20) to calculate the velocity of 
body C at each integration time step, drifting of the bodies is eliminated, as is the associated compu- 
tational instability. 

It would be sufficient to stop at this point because the stated intention of preventing compu- 
tational instability has been accomplished. H owe ver, one more step is added to the process. The 
translational position of body C will be determined in the same manner as was its velocity. 
Specifically, a position constraint equation will be used, instead of integration, to define the transla- 
tion of the CM of body C at each integration time step. The two bodies will thus not only maintain the 
proper relationship of their velocities but their positions as well. Details of the position constraint 
are presented in the computer program listing in the appendix. 
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APPENDIX 


This appendix contains a digital computer program listing of the two-body simulation which 
uses the programming technique that eliminates computational instability associated with the 
Lagrange multiplier method. The two bodies are connected by a frictionless swivel. The aero- 
dynamics and certain other aspects of the simulation are somewhat simplified to allow clearer illus- 
tration of the basic principles used. No subroutines are listed because they all perform relatively 
simple calculations such as matrix multiplication, trigonometric functions with zero denominators, or 
integration. The program listing, in FORTRAN, begins on the next page. 
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JAN 15, 1992 
GAINES WATTS 
M3FC - ED 1 3 

*####■»*- **•##■))•*•*** *##£*#»*##*##$£*£*###•«■*##**■*■*#* 


TTTTTTT WWW 00000 BE’BEBB 00000 DDDDDD Y Y . 

T W W W 0 0 B BO 0 D DYY * 

T W W W 0 0 BBBBBB 0 0 D D Y 

T WWWO OB BO OD D Y 

T WW WW 00000 BBBBBB 00000 DDDDDD Y j 

* ## #### ## ## ## *### *# *#######** ** ft#####*#**## **"*'* 

1 

A SIMPLE TWO-BODY LAGRANGE MULTIPLIER SIMULATION TO ILLUSTRATE I 

THE PROGRAMMING TECHNIQUE THAT ELIMINATES COMPUTATIONAL INSTABILITY I 
CAUSED BY DRIFTING APART OF THE BODIES. j 

1 

«•**#*#£•*«■#### *•»■ *###*#■*£#**•&•»*•»**##*#*■##*£##*$•*#•&*•*#■•## a###-#### ] 


PROGRAM INTRODUCTORY COMMENTS? 

1. THE TWO BODIES IN THIS PROGRAM ARE AN SRB AND A PARACHUTE, ! 

DESIGNATED BY SUBSCRIPTS "B", AND "C", RESPECTIVELY. A 
FRICTIONLESS SWIVEL IS USED TO CONNECT THEM. 

2. BOTH BODIES ARE ASSUMED TO BE RIGID WITH NO CHANGES IN 
MASS PROPERTIES DURING A SIMULATION RUN. NO ADDED MASS, 

APPARENT MASS, OR AERODYNAMIC DAMPING ARE CONSIDERED. 

3. A FLAT EARTH WITH CONSTANT GRAVITATIONAL ACCELERATION IS 
ASSUMED. ATMOSPHERIC DENSITY VARIES WITH ALTITUDE, BUT NO 
WINDS ARE USED. 

4. THE INERTIAL REFERENCE IS ASSUMED TO BE THE 'I' FRAME WHICH IS 
LOCATED AT THE EARTH'S SURFACE DIRECTLY BENEATH THE INITIAL 
POSITION OF THE SRB. THE ORIENTATION OF THE 1 I ' FRAME 

IS? NORTH - EAST- DOWN. 

5. THE ’ B ' AND ’C' FRAMES ARE FIXED AT THE C.M. OF THE SRB AND 
PARACHUTE, RESPECTIVELY. EACH FRAME USES THE X-AXIS AS THE 
ROLL AXIS. 

6. TWO SETS OF 3-2-1 EULER ANGLES RELATE THE ’ B ’ AND ’C' 

FRAMES TO THE ’ I ' FRAME. f 

7. AERO COEFFICIENT TABLES FOR THE SRB AND THE PARACHUTE ARE 

EXPRESSED IN "MISSILE" AXES AS A FUNCTION OF THE TOTAL 
ANGLE OF ATTACK. A POSITIVE ROTATION ABOUT THE ROLL AXIS 
IS USED TO TRANSFORM FROM "MISSILE" AXES TO THE BODY-FIXED 
FRAME FOR THE SRB OR PARACHUTE. 3 

m 

8. THE MOMENT REFERENCE POINT (M.R.P.) FOR EITHER BODY IS AT 1 

THE C.M. • ->■ * " - I 
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0 0 0 0 0 0 0 0 


9. A 'V' FRAME IS USED FDR THE SRB , HAVING ITS X-AXIS ALONG THE 
SRB VELOCITY VECTOR AND ITS Y-AXIS HORIZONTAL. 

10. A FLAG IS AVAILABLE TO ZERO OUT THE EXTERNAL FORCES AND 

MOMENTS, AND PERMIT THE ATTACH POINT CONSTRAINT FORCES AND 
MOMENTS TO BE CHECKED. (SET IFE = 0 TO USE THIS OPTION). 


C# *#**#*#**###*#**■****** 




c 


c 


c 


C 


C 


C 


c 


c 


PARAMETER (NAB = 9) 

PARAMETER (NAC = 5) 

PARAMETER ( NRHO = 8) 

PARAMETER ( NRNK = 30) 

REAL LABB (3) ,LACC(3) »LABI (3) ,LACI (3) , LREFB , LREFC 

REAL MASSE , MAS SC , I XXB , I X XC , I YYB , I Y YC » I ZZB, I Z ZC , I XYB, I X YC , 

2 1 X ZB , I XZC » IYZB , IYZC 

REAL I MB (3,3) , IMC(3,3) ,IMBINV(3,3) ,IMCINV(3,3) 

REAL M (12, 12) , MINV (12, 12) , MI DENT <12, 12) 

DIMENSION ABI (3,3) ,ACI (3,3) , AIB (3, 3) ,AIC(3,3) , ACB (3, 3) 
DIMENSION ABV ( 3, 3) ,AVI(3,3),AIV<3,3) 

DIMENSION A (3, 12) , AT ( 12» 3) , ADOT (3, 12) , AMINV (3, 12 ) , AMINVA (3 ,3) , 
2TERMA1 (3 ,3) , AM INVF (3 ) , ADDTOM (3) , TERM VI (3) , TERMV2 (3) , ADOMEG (3) 


DIMENSION OMEGA ( 12) ,D0MEGA(12) ,D0M9<9) 

DIMENSION VIBB (3) ,VICC<3) r VI El (3) ,VICI (3) ,V0BB(3) ,V0CC(3) , 
2VABB (3) , VABC ( 3 ) , RIBI (3) , RABI (3) , RICI (3) , OMB (3) , OMC (3) 
DIMENSION FL ( 1 2) ,FG(12),FE(12) ,FW(12),F(12),FA(12) ,FT0TAL(12) 
DIMENSION GI (3) ,GB(3) ,GC(3) > H < 12 ) 


DIMENSION ALBTL(NAB) ,CAMBTL(NAB) , CNMBTL (NAB) ,CMMBTL (NAB) 
DIMENSION ALCTL(NAC) , C AMCTL ( NAC) , CNMCTL ( NAC) ,CMMCTL(NAC) 
DIMENSION CARD (20) ,T ITLE ( 1 6) , R HO ALT (NRHO) , RHOTL ( NRHO ) 
DIMENSION XXD(NRNK) , XX (NRNK) 


EQUIVALENCE 
EQUIVALENCE 
EQUIVALENCE 
EQUI VALENCE 
EQUIVALENCE 
EQUIVALENCE 
EQUIVALENCE 
EQUIVALENCE 


(XXD(Ol) , B 1 1 D ) , (XXD(02) ,B12D) , (XXD(03) ,B13D) 
(XXD (04) ,B21D) , (XXB(05) ,B22B) , (XXD (06) ,B23D) 
( X XD ( 07 ) , B31 D) , ( XXD ( 08 ) ,B32D) , (XXD(09) ,B33D) 
(XXD(IO) ,C11D) ,(XXD(11),C12D), ( X XD ( 1 2 ) , C 1 3D ) 
(XXD (13) ,C21D) , (XXD (14) ,C22D) , (XXD (15) ,C23D) 
(XXD (16) , C31D) , (XXD (17) ,C32B) , (XXD (18) ,C33D) 
(XXD (19) , D0M9 ( 1 ) ) 

(XXD (28) , V IB I ( 1) ) 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


( X X ( 01 ) , B1 1 ) , ( XX (02) , B 1 2 ) , (XX (03) , B1 3) 

(XX (04) ,E21) , (XX (05) ,B22) , (XX (06) ,E23) 

(XX (07) ,B31) , (XX (08) , B32) , (XX (09) ,B33) 

( X X ( 1 0 ) , C 1 1 ) , (XX (1 1) , C 1 2 ) , ( X X ( 1 2 ) , C 1 3 ) 

(XX ( 13 ) , C21 ) , ( XX ( 1 4) , C22 ) , (XX ( 15) »C23) 

(XX ( 16) ,C31> , (XX (17) ,C32> , (XX ( 18) ,C33) 

(XX ( 19) , VIBB (1) ) , ( XX (22) ,0MB (1 ) ) , (XX (25) , OMC ( 1 ) ) 
( X X ( 28 ) ,RIBI (1)) 
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O O O DO o o o o o o o 


C AERO COEFFICIENT TABLES IN MISSILE AXES FOR THE SRB AND THE 

C PARACHUTE AS A FUNCTION DF TOTAL ANGLE OF ATTACK. 

C 

C CA - - AXIAL FORCE 

C CN - - NORMAL FORCE 

C CM - - PITCH MOMENT 

C CY - - SIDE FORCE 

C CW YAW MOMENT 

C CR - - ROLL MOMENT 

C 

DATA ALBTL/O . , 20 . , 40 . , 60 . , 90 . , 120. , 1 47 . , 160. , 1 SO . / 

DATA CAMBTL/O.O, 0.0, 0.0, +0.50, +1 .40,-1 . 35 , -3 . 65 , -3 . 00 , -2 . 20/ 
DATA CNMBTL/0.0»0.0»0.0»+9.00,+9.00»+9.00,+4.10,+1 .80, +0.00/ 
DATA CMMBTL/O. 0,0. 0,0.0, +15. 5, +5. 00,-3. 30,-1. 90,-1 .10, +0.00/ 
CYMB = 0.0 
CWMB = 0.0 
CRMB =0.0 
C 

DATA ALCTL/O. ,5. ,10. ,15. ,25./ 

DATA CAMCTL/ +0.62, +0.62, +0.62, +0.61 ,+0.56/ 

DATA CNMCTL/. 000, +.0100, +.032, +.090,+. i70/ 

DATA CMMCTL/. 000, -.0133, -.037, -.091,-. 175/ 

CYMC = 0.0 
CWMC = 0.0 
CRMC = 0.0 

# * 

REFERENCE AREAS AND LENGTHS FOR THE SRB AND PARACHUTE. 

SREFB = 116.26 
LREFB = 12.1667 
SREFC = 2289.1 
LREFC = 54.00 

ft*#**###*# 

ATMOS. DENSITY TABLE - - ENGLISH SYSTEM. 

DATA RHOALT/OO . , 5000 ., 8224 ., 8803 . , 14000. ,20000. ,30000. ,40000./ 
DATA RHOTL/O. 0022964, 0.001 9909, 0.001 8053, 0.001 7735, 

BO. 00 15086, 0.00 12483, 0.0009024, 0.0006306/ 

# # * # * * # * * # 

CONSTANTS. 

CNV = 57.2957795 
GZERO = 32.1740485 

READ AND PRINT DESIRED PORTION OF FORTRAN SOURCE DATASET. 

READ (5,805) NPM IN , NPMAX 
DO 100 1 = 1 , NPMAX 
READ (9,820, END=105) CARD 
IF (I .LT. NPMIN) GO TO 100 
WRITE (6, 822) CARD 
100 CONTINUE 
105 WRITE (6,800) 
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READ AND WRITE THE INPUT DATA. 


READ (5,301) 

WRITE(6, 802) 

READ (5,307) 

WRITE<6, 803) 

C 

READ (5,807) 

WR ITE ( 6 , SOS) 

READ ( 5 , S07) 

WR ITE ( 6 , SOS) 

READ (5,807) 

WRITE<6, 808) 

C 

READ (5,307) 

WR ITE ( 6 , SOS ) 

READ (5,807) 

WRITE ( 6 , SOS) 

READ (5,807) 

WRITE ( 6, SOS) 

C 

WR ITE ( 6 , 302) 

MASSE = WGHTE/GZERO 
MASSC = WGHTC/GZERO 
C 

c* ####*#**- *******£#**•*##*****#■#•*#* 

C SET UP THE CM3 MATRIX AND ITS INVERSE. CM3 IS A 12X12 WHICH CAN 

C EE TREATED AS A QUASI -DIAGONAL MATRIX WITH FOUR 3X3 CELLS. 

C THE INVERSE OF CM3 CAN EE FOUND BY SIMPLY TAKING THE INVERSE 

C OF EACH CELL. 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 

— 0 0 0 
I 

10 0 0 
I 

— 0 0 0 

0 

I I 

01 IMC I 
I I 

0 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


0 


CM3 = 


I 


i MASSE 10000 
I I 

0 0 0 0 


0 

0 

0 

0 

0 

0 


0 

0 


0 

0 


0 0 
0 0 


0 

0 


0 0 
0 0 


0 
0 
0 
0 

0 0 


I MB 10 
1 

0 

0 0 0 — 


0 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 


1 

0 0 I MASSC 

I 

0 0 


0 

0 

0 


0 0 0 
0 0 0 
0 0 0 


TITLE, IDIAG,IFE 
TITLE, IDIAG, IFE 

TSTART , TMAX , DELT , DTF'RT , TNP1 , TNF'2 
TSTART , TMAX , DELT, DTPRT , TNP1 , TNF’2 

VIES, AZ I IBS, GAM IBS, ALTBS 
VIES, AZ1 IBS, GAM IBS, ALTBS 

PDEGBS , QDEGBS , RDEGBS , ALPHBS , PHI AES , BANK BS 
F'DEGBS , QDEGBS , RDEGBS , ALPHBS ,PHI ABS , EANKBS 
PDEGCS,QDEGCS,RDEGC5,PSICS,THETCS,PHICS 
F'DEGCS , QDEGCS , RDEGCS , PS ICS, THETCS , PHI CS 

WGHTB » I XXB, iyyb » i zzb, ixyb,ixzb, iyzb 
WGHTB , I XXB , IYYB,IZZB, IXYB,IXZB, IYZB 
WGHTC , I XXC , IYYC, IZZC, IXYC, IXZC, IYZC 
WGHTC , I XXC, IYYC, IZZC, IXYC, IXZC, IYZC 
XATTB, YATTB , ZATTB , XATTC , YATTC , ZATTC 
XATTB , YATTB , ZATTB , XATTC , YATTC , ZATTC 
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FIRST, ZERO ALL THE ELEMENTS OF CM] AND CMINV]. 

DO 121 1=1,12 
DO 120 J = 1 , 12 
MCI, J) = 0.0 

120 MINVCI , J) =0.0 

121 CONTINUE 

NOW, SET UP THE MASS CELLS IN CM] AND CMINV]. EACH MASS CELL IS 
A DIAGONAL MATRIX. 

DO 130 1=1,3 
MCI, I) = MASSE 

MC 1+6, 1+6) = MASSC 
MINVCI, I) = l./MASSB 
130 MINV (1+6, 1+6) = 1. /MASSC 

NEXT, SET UP EACH INERTIA MATRIX AND ITS INVERSE. 

CALL IMATRX ( I XXB , I Y YB , I ZZB , I X YB , I XZB , I YZB , I MB ) 

CALL IMATRX < I XXC, I YYC , I ZZC, IX YC , I XZC, IYZC, IMC) 

CALL MAINV3 (1MB , IMBINV) 

CALL MAINV3 (IMC.IMCINV) 

FINALLY, SET UP THE INERTIA CELLS IN CM] AND CMINV]. 

DO 133 1=1,3 
DO 132 J=1 ,3 
MC 1 + 3, J + 3) = I MB (I , J) 

M ( 1 + 9 , J + 9) = I MC ( I , J ) 

MINV (1+3, J + 3) = IMBINV Cl , J> 

132 MINV (1+9, J+9) = IMC I NV ( I , J ) 

133 CONTINUE 
£ # 

CHECK THE INVERSE OF CM] EY PRINTING CM] AND C M ]C MINV ] . 

# * 

DO 141 1=1,12 

141 WRITE (6,312) (MCI, J),J= 1,12) 

CALL MMUL ( M , M IN V , M I DENT , 1 2 , 1 2 , 1 2) 

WRITE (6,302) 

DO 142 1=1,12 

142 WRITE (6,312) (M I DENT ( I , J) , J = 1 , 1 2) 

f- ■»*■###*** ###£■#•**•*• **•&#**•#*#* ******* 

SET UP VECTORS FOR C . M . -TO-ATT ACH-PO INT DISTANCES. BOTH VECTORS 
ARE DEFINED AS POSITIVE FROM C.M. TO ATTACH POINT. 

LABE ( 1 ) = XATTB 
LABE (2) = YATTB 
LABE (3) = ZATTB 
LACC(l) = XATTC 
LACC (2) = YATTC 
LACC ( 3 ) = ZATTC 


***************** 

SET UP THE 3RD AND 4TH CELLS OF C A ] , THE 3X12 CONSTRAINT 
MATRIX WHICH HAS THE GENERAL FORM: 
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[ A j I + ACB I -ACB*LTB I -IDENT I +LTC I 


WHERE LTB AND LTC ARE TILDE MATRICES FOR THE LABB AND 
LACC VECTORS. 

FIRST, ZERO ALL THE ELEMENTS OF C A ] - ZERO C ADOT I ALSO. 

DO 173 1=1,3 
DO 172 J=1 , 12 
A ( I , J) =0.0 

172 ADOT (I , J)= 0.0 

173 CONTINUE 

NOW, FILL IN NON-ZERO ELEMENTS IN THE 3RD AND 4TH CELLS OF [A3. 
A(l, 7) = -1.0 
A ( 2 , S) = -1.0 
A ( 3 , 9) =-1.0 
Ad, 11) = -LACC ( 3) 

Ad , 12) = +LACC ( 2) 

A ( 2, 10) = +LACC ( 3) 

A ( 2, 12) = -L ACC ( 1 ) 

A ( 3 , 10 ) = -LACC ( 2 ) 

A ( 3 , 1 1 ) = +LACC < 1 > 

*•»**##* #*#■#* 

BEGIN INITIALIZATION SECTION IN WHICH THE VALUES OF ALL NEEDED 
PARAMETERS ARE CALCULATED AT TIME = TSTART . 

TIME = TSTART 

ESTABLISH THE SRB PARAMETERS AT TIME = TSTART BY SETTING THEM 
EQUAL TO THE VALUES ALREADY READ IN. 

VI B = VI BS 
AZIIB = AZ I IBS 
GAMIB = GAMIBS 
ALTB = ALTBS 
PDEGB = PDEGBS 
QDEGB = QDEGBS 
RDEGB = RDEGBS 
ALPHAB= ALPHBS 
PHIAB = PHIABS 
BANKB = BANKBS 

DETERMINE 2 MATRICES TO INITIALIZE C AE 13, THE 'I’ FRAME TO T B 7 
FRAME TRANSFORMATION. - - CABII = C ABV ][ AV I 3 

# 

SET UP [ABV 3 USING A 1-2-1 EULER ANGLE SEQUENCE ( BANKB , ALPHAS , 
PHIAB) . 

SB KB = SIN (BANKB/CNV) 

CBKB = COS (BANKB/CNV) 

SALB = SIN (ALPHAB/CNV) 

CALE = COS ( ALPHArB/ CNV) 
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SPAB = SIN (PHI AB/CNV) 

CRAB = COS (PHI AB/CNV) 

ABV ( 1 , 1) = CALB 
ABV (1,2) = SBKB*SALB 
ABV < 1 , 3) =-CBKE*SALB 
ABV O » 1) = SPAB*SALB 

ABV (2,2) = CPAB*CBKB - SPAB*SBKB*CALB 
ABV ( 2 , 3 ) = CPAB*SBKB + SPAB*CBKB*CALB 
ABV (3,1) = CPAB*SALB 
ABV (3,2) =-SPAB*CEKB - CPAB*SBKB*CALB 
ABV (3,3) =-SPAB*SBKB + CPAB*CBKB*CALB 

C # * 

C SET UP [AVI 3. 

SAZB = SIN (AZI IB/CNV) 

CA ZB = COS (AZI IB/CNV) 

SGMB = SIN (GAHIB/CNV) 

CGMB = COS (GAMIB/CNV) 

AVI ( 1 , 1 ) = CAZB*CGMB 
AVI ( 1 , 2) = SAZB* CGMB 
AV I ( 1 , 3) =-SGMB 
AVI (2,1) =-SAZB 
AVI (2,2) = CAZB 
AVI (2,3) = 0.0 
AVI (3,1) = CAZB*SGMB 
AVI (3, 2) = 5AZB*5GMB 
AVI (3,3) = CGMB 

C * # 

C CALC. LABI 3 AND SET UP ITS ELEMENTS AT TIME = TSTART . 

CALL MMUL ( ABV , AV I , ABI , 3 , 3 , 3 ) 

B1 1 = AB I (1,1) 

B1 2=AB I (1,2) 

B1 3=AB I ( 1,3) 

' B2 1 = AB I ( 2 , 1 ) 

E22= AB I (2,2) 

B23=AB 1(2,3) 

B3 1 =AB I (3,1) 

B32=ABI (3,2) 

B33=AB I (3,3) 

C * # 

C CALC. THE SRB INERTIAL POSITION COMPONENTS IN THE 

C ' I ' FRAME AT TIME = TSTART. 

RIBI (1 ) = 0.0 
RIBI (2) =0.0 
RIBI (3) = -ALTB 

C * # 

C CALC. THE SRB INERTIAL VELOCITY COMPONENTS IN THE ’I' FRAME 

C AND TRANSFORM TO THE 'B’ FRAME AT TIME = TSTART. 

VIBI(l) = VIB*CGMB*CAZB 
VIBH2) = V IB#CGMB*SAZB 
VI El (3) = -V I B*SGMB 
CALL MATVEC (ABI , V IB I , VI BB , 3 , 3 ) 

C 

C SET UP THE SRB ANGULAR VELOCITY COMPONENTS AT TIME = TSTART. 

OME(l) = PDEGB/CNV 
OME ( 2) = QDEGB/CNV 
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OME ( 3 ) 


RDEGB/CNV 


C 

f* # .£ # # •£ # 

C ESTABLISH THE PARACHUTE PARAMETERS AT TIME = TST ART BY SETTING 

C THEM EQUAL TO THE VALUES ALREADY READ IN. 

PSIC = PSICS 
THETC = THETCS 
PHIC = F'HICS 
PDEGC = PDEGCS 
QDEGC = QDEGCS 
RDEGC = RDEGCS 

c # # 

C SET UP THE PARACHUTE ANGULAR VELOCITY COMPONENTS AT TIME = TST ART . 

OMC(l) = PDEGC/CNV 
0MC(2) = QDEGC/CNV 
OMC ( 3) = RDEGC/CNV 

Q # 

C SET UP C AC I 3 USING 3-2-1 EULER ANGLE SEQUENCE (PSI C , THETC, PHIC ) . 

ST3C = SIN (PSIC/CNV) 

CT3C = COS (PSIC/CNV) 

ST2C = SIN (THETC/CNV ) 

CT2C = COS (THETC/CNV) 

STIC = SIN (PHI C/CNV) 

CT 1C = COS (PHI C/CNV) 

AC 1(1,1) = CT2C*CT3C 
ACK1, 2) = CT2C*ST3C 
ACK1, 3) =-ST2C 

AC 1(2,1) = ST1C*ST2C*CT3C - CT1C*ST3C 
ACI (2,2) = ST1C*ST2C*ST3C + CT1C*CT3C 
AC I (2,3) = ST1C*CT2C 

ACI (3,1) = CT1C*5T2C*CT3C + ST1C*ST3C 
ACI (3,2) = CT1 C*ST2C*ST3C - ST1C*CT3C 
ACI (3,3) = CT1C*CT2C 

c # # 

C SET UP THE ELEMENTS OF C ACI 3 AT TIME = TSTART. 

Cl 1 = ACI ( 1,1) 

Cl 2=AC 1(1,2) 

Cl 3=ACI (1,3) 

C21=ACI (2, 1 ) 

C22= AC I (2,2) 

C23=ACI (2,3) 

C3 1=AC I (3,1) 

C32= AC I (3,2) 

C33= ACI (3,3) 

£ # £ 

C SET INITIAL VALUES OF PRINT PARAMETERS AND OTHER PARAMETERS. 

TPRINT = TSTART 
IPRFLG = 0 
ICOFLG = 0 
KUTTA = 4 
C 

C INITIALIZATION COMPLETED ; PRINT HEADER INFORMATION. 

WRITE (6,800) 

WRITE (6,802) TITLE 
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WRITE (6,808) VI BB ( 1 ) , VI BB (2 > , V I BB < 3 > , VI Bl (3 > , X X D ( 30 > , X X ( 30 > 

WRITE (6,802) 

GO TO 407 

C 

C BEGIN INTEGRATION LOOP (INTEGRATION SUBR NOT CALLED FIRST PASS). 

C***tt***«tf*««****«#tt**«*#ft*tf***#'**«* 

C 

400 KUTTA = KUTTA + 1 

CALL RUNGF ( NRNK , DELT , TI ME , X XD , X X , KUTTA) 

407 CONTINUE 

C * * 

C CALCULATE ELEMENTS OF [ ABI DOT 3 AND CACIDOT]. 

BUD = B21*0MB(3) - B31*DMB(2) 

Bl 2D = B22*0MB(3) - B32*0MB(2) 

Bl 3D = B23*0MB(3) - E33*0MB(2) 

B21D = B31 *OMB ( 1 ) - B11*0MB(3) 

B22D = B32#0MB ( 1 ) - B12*0MB(3> 

B23D = B33*0MB(1) - B13#0MB(3) 

B31D = E11*GME(2) - B21*0MB(1) 

B32D = B 1 2 * 0 MB ( 2 ) - B22#0MB(1) 

B33D = B 1 3*0 MB ( 2 ) - B23*0MB(1) 

C 

Cl ID = C21 *OMC (3) - C31*0MC ( 2) 

Cl 2D = C22*0MC(3> - C32#0MC(2) 

Cl 3D = C23*0MC ( 3 ) - C33*0MC(2) 

C21D = C3 1 *0MC ( 1 ) - C11*0MC(3) 

C22D = C32*0MC ( 1 ) - C12*0MC(3> 

C23D = C33*0MC ( 1 ) - C13*0MC(3> 

C31D = Cl 1 *OMC (2 ) - C2 1 *0MC ( 1 ) 

C32D = C12*DMC(2) - C22*0MC<1) 

C33D = C 1 3*QMC (2 ) - C23*0MC(1) 

C * # 

C SET UP THE C ABI D AND EACH MATRICES. 

ABI ( 1 , 1) =B1 1 
ABI ( 1 ,2) =B 1 2 
ABI ( 1 , 3) =B13 
ABI (2, 1) =B21 
ABI (2, 2) =B22 
ABI (2,3) =B23 
ABI (3, 1 ) =B3 1 
ABI (3,2) =B32 
ABI (3, 3) =B33 

C 

AC I ( 1 , 1) =C11 
AC I (1,2) =C 12 
AC I ( 1 , 3) =C13 
AC I (2, 1) =C2 1 
AC I (2,2) =C22 
AC I (2,3) =C23 
AC I (3, 1) =C31 
AC I ( 3 , 2) =C32 
AC I (3,3) =C33 

C * # 

C CALCULATE CAIB3.CAIC3, AND CACB3. 
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CALL TRANSP ( ABI , 3 , 3 , A IB ) 

CALL TRANBP (ACI ,3 ,3,AIC) 

CALL MMUL (ACI , AIB , ACB , 3 , 3 , 3 ) 

USE CONSTRAINT EQUATIONS TO DETERMINE THE INERTIAL POSITION AND 
VELOCITY OF THE PARACHUTE , ELIMINATING DRIFT CAUSED BY 
INTEGRATION ERROR. 

THE CONSTRAINT EQUATIONS WILL EE CALCULATED USING A VECTOR- 

CROSS-PRODUCT SUBROUTINE INSTEAD OF TILDE MATRICES. ALLOWING 
A LITTLE MIXING OF MATRIX AND VECTOR NOTATION, THE CONSTRAINT 
EQUATIONS ARE? 

RICI = RIEI + [ AIB ]*LABB - CAIC]*LACC 

VICC = CACB]*(VIBB + OMB X LABB) - OMC X LACC 

# * # * 

BEGIN WITH POSITION CONSTRAINT. 

# # # # 

TRANSFORM SRB C . M . -TO-ATTACH POINT DISTANCE INTO ’ I ' FRAME. 

CALL MATVEC ( A IB , LABB , LABI , 3 , 3 ) 

CALC. INERTIAL POSITION OF THE ATTACH PDINT IN THE ’ I ' FRAME. 

CALL VADD (R IB I , LABI ,RABI , 1 ) 

TRANSFORM PARACHUTE C . M . -TO-ATTACH POINT DISTANCE INTO ’ I ’ FRAME. 

CALL MATVEC ( A IC , L ACC , LAC I , 3 , 3 ) 

CALC. INERTIAL POSITION OF PARACHUTE C.M. IN THE ’I’ FRAME. 

CALL VADD (RABI , LACI ,RICI , -1 > 

# £ $ # 

NOW CALCULATE THE VELOCITY CONSTRAINT. 

£ # # 

DETERMINE ATTACH POINT VELOCITY RELATIVE TO THE SRB C.M. IN THE 
’B' FRAME. 

CALL VCROSS (OMB , LABB , VOBB ) 

CALC. INERTIAL VELOCITY OF THE ATTACH POINT IN THE ’ B T 
FRAME AND TRANSFORM INTO ’C 7 FRAME. 

CALL VADD (V IBB , VOBB , VABB , 1 ) 

CALL MATVEC (ACB , VABB, VABC ,3,3) 

DETERMINE ATTACH POINT VELOCITY RELATIVE TO THE PARACHUTE C.M. IN 
THE T C 7 FRAME. 

CALL VCROSS (OMC , LACC , VOCC ) 

CALC. INERTIAL VELOCITY OF THE PARACHUTE C.M. IN THE T C 7 FRAME. 
(THIS VELOCITY WILL BE PLACED IN THE IOMEGA] MATRIX). 

CALL VADD ( VABC , VOCC , V ICC , -1 ) 

TRANSFORM INERTIAL VELOCITY OF THE SRB INTO ' I 7 FRAME TO PROVIDE 
A DERIVATIVE OF INERTIAL POSITION. DO THE SAME FOR PARACHUTE. 
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CALL MAT VEC ( A IB , V I BE , VI BI , 3 , 3 > 
CALL MATVEC ( A I C , V I CC , VI Cl , 3 , 3 ) 
C 


c 

C BEGIN AERO SECTION. 

C 

C * # # * 

C LOOK UP ATMOSPHERIC DENSITY USING SRB ALTITUDE. 

ALTB = -RIBI (3) 

ALTC = -RICI (3) 

CALL TBLXY ( NRHO , RHOALT , RHOTL , ALTB , RHO) 

C * # 

C CALCULATE DYNAMIC PRESSURES FOR SRB AND PARACHUTE. ALONG WITH 

C ASSOCIATED TERMS FDR AERO CALCULATIONS. 

C 

CALL VMAG (VIBB.VIB2.VIB) 

CALL VMAG ( VICC , VI C2 , VIC) 

QBRE * 0 . 5#RHG#V IB2 
QBRC = 0 . 5*RH0*VIC2 
QBRSB = QBRB*SREFB 
QBRSC = QBRC*SREFC 
QBRSLB = QBRSB*LREFB 
QBRSLC = QBRSC*LREFC 

C # * 

C CALCULATE ALPHA-TOTAL AND AERO ROLL ANGLE FDR THE SRB 

C AND PARACHUTE. ALPHA-TOTAL HAS RANGE: 0 TO ISO DEGREES. 

C AERO ROLL ANGLE HAS RANGE: -ISO TO +1SO DEGREES. 

C 


c 

c 


c 


c 

c 

c 

c 


c 


ALPHAB = ACOS(VIBBU) /VIB)*CNV 
ALPHAC = ACOS <VICC( 1) /VIC)*CNV 
F’HIAB = ZTAN2 (VIBB<2) ,VIBB<3> )*CNV 
PH I AC = ZTAN2 (VICC(2) ,VICC<3) >*CNV 

LOOK UP SRB AND PARACHUTE AERO COEFFICIENTS IN MISSILE AXES. 
CALL TBLXY ( NAB , ALBTL , CAMBTL , ALPHAB , CAMB) 

CALL TBLXY ( NAB , ALBTL , CNMBTL , ALPHAB , CNMB) 

CALL TBLXY ( NAB , ALBTL , CMMBTL , ALPHAB , CMMB ) 

CALL TBLXY ( NAC , ALCTL , CAMCTL , ALPHAC , CAMC ) 

CALL TBLXY ( NAC , ALCTL , CNMCTL , ALPHAC . CNMC ) 

CALL TBLXY (NAC, ALCTL, CMMCTL .ALPHAC. CMMC) 

TRANSFORM AERO COEFFICIENTS FOR THE SRB AND PARACHUTE INTO THE 
T B' AND ’C* FRAMES. 

SF’AB = SIN (PHI AB/CNV > 

CRAB = COS (PHI AB/CNV) 

CAB = +CAMB 

CYB = +CYMB*CPAB - CNMB*SPAB 

CNB = +CNMB*CPAB + CYMB*SPAB 

CRB = +CRMB 

CMB = + CMMB*CF'AB + CWMB*SPAB 

CWB = +CWMB*CPAB - CMMB*SPAB 
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c 

SPAC = SIN (PHI AC/CNV ) 

CF' AC = COS (PHI AC/CNV ) 

C 

CAC = +CAMC 

CYC = + CYMC*CF'AC - CNMC*SPAC 

CNC = +CNMC*CPAC + CYMC*SPAC 

CRC = + CRMC 

CMC = +CMMC#CPAC + CWMC*SPAC 

CMC = +CWMC*CPAC - CMMC*SPAC 

* * 

CALC. AERO FORCES AND MOMENTS AT EACH C.M., TAKING INTO ACCOUNT 
THE SIGN CHANGE REQUIRED FOR CA AND CN. 

FLU) =.-QBRSB*CAB 
FL (2) = +QERSB*CYB 

FL (3 ) = -QBRSB*CNB 

FL (4 ) = QERSLB#CRB 

FL (5 ) = QERSLB*CMB 

FL ( 6 ) = QBRSLB#CWB 

FL (7) = -GBRSC*CAC 

FL (8 ) = +QBRSC*CYC 

FL ( 9 ) = -QBRSC*CNC 

FL(10) = GBRSLC*CRC 
FL(il) = QBRSLC*CMC 
FL (1 2) = QBRSLC*CWC 

END OF AERO SECTION. 

SET UP GRAV. ACCELERATION COMPONENTS IN THE ’I' FRAME. 

GI (1) = 0.0 

GI (2 ) = 0.0 

G I ( 3 ) = G2ER0 

* * 

TRANSFORM THE GRAV. ACCELERATION TO THE ’B’ AND ’C' FRAMES , 

AND CALC. THE GRAV. FORCES AT EACH C.M. - - C FG 3 MATRIX. 

CALL MATVEC ( ABI , GI , GB ,3 ,3) 

CALL MATVEC (AC I , GI » GC , 3 » 3) 

DO 460 1=1,3 
'FG(I) = MASSB*GB(I) 

FGU+3) = 0.0 
F G ( I + 6 ) = MASSCfrGC (I ) 

460 FGU+9) = 0.0 

# # * * # 

IF ’ IFE’ EQ 0, ZERO OUT THE TERMS THAT COMPRISE THE E FE 3 MATRIX. 

IF ( I FE .NE. 0) GO TO 481 
DO 480 1=1,12 
FLU) = 0.0 
480 FG(I) = 0.0 
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481 CONTINUE 


C 

C ALL THE EXTERNAL FORCES AND MOMENTS HAVE NOW BEEN CALCULATED AT 

C THE SRB AND PARACHUTE C.M.’S. SET UP THE [ FE ] MATRIX. 

DO 490 1=1 , 1 2 
490 FE ( I ) = FL ( I ) + FG ( I ) 

C 

C SET UP THE IOMEGA] MATRIX USING THE PARACHUTE INERTIAL VELOCITY 

C FROM THE CONSTRAINT EQUATION. 

DO 512 1=1,3 

OMEGA ( I ) = VI BBC I) 

OMEGA (1+3) = OMBCI) 

OMEGA (1+6) = VICC(I) 

512 OMEGA ( 1+9) = OMC(I) 

C 

C * * * * * 

C CALCULATE THE MOMENTUM MATRIX [H] USING THE 12X12 MASS MATRIX. 

CALL MATVEC (M , OMEGA * H , 1 2, 12) 

C 

C * * * 

C SET UP THE [FW] MATRIX BY CALCULATING THE 'MINUS W-CROSS' TERMS 

C FOR THE SRB AND THE PARACHUTE- 

r 


FW (1 ) 

- 

-OMB (2)*H(3) 

+ 

OMB (3) *H (2) 

FW (2) 

S 

-OMB (3) *H ( 1 ) 

+ 

OME ( 1 ) #H (3 ) 

FW (3) 

= 

-OMB ( 1 ) *H ( 2) 

+ 

OMB (2) *H < 1 ) 

FW (4) 

— 

-OMB (2 J *H (6) 

+ 

OMB ( 3) *H (5) 

FW (5) 

= 

-OMB ( 3 ) * H ( 4 > 

+ 

OMB ( 1 ) #H (6) 

FW (6) 

— 

-OMB ( 1 ) *H ( 5) 

+ 

OMB ( 2) #H (4 ) 

FW (7) 

S 

-OMC (2)*H(9) 

+ 

OMC ( 3) #H (8 > 

FW (8) 

= 

-OMC (3 ) *H ( 7) 

+ 

0MC(1)*H<9) 

FW (9) 

= 

-OMC (1 )*H(S) 

+ 

OMC ( 2) #H (7 ) 

FW (10) 

35 

-OMC (2 ) *H ( 12) 

+ 

OMC ( 3) *H ( 1 1 ) 

FW (1 1) 

= 

-OMC (3) *H ( 10) 

+ 

OMC ( 1 ) #H ( 1 2) 

FW (12) 

= 

-OMC (1 )*H( 11 ) 

+ 

OMC ( 2) *H ( 1 0) 


C 

C ADD [FW] TO THE EXTERNAL FORCES AND MOMENTS TO GET 

C THE [F] MATRIX. ? 

DO 524 1=1,12 ! 

524 F ( I ) = FE ( I ) + FW ( I > ; 

C - 

C*##****-£*####**tt£*#***#* 

C BEGIN CALCULATION OF ATTACH POINT CONSTRAINT FORCES AND ! 

C MOMENTS AT THE C.M.'S [FA] MATRIX. 

C # # 1 

C SET UP THE 1ST AND 2ND CELLS OF [A], THE CONSTRAINT MATRIX. THE Z 

C 1ST CELL IS CAGE]. THE 2ND CELL, -[ ACB ][ LTB ] » IS OBTAINED I 

C BY USING THE 1ST CELL. 

c : 
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DO 533 1 = 1 , 3 
DO 535 J=1 ? 3 

532 A(I-J) = ACB (I ,J) 

533 CONTINUE 

A ( 1 , 4) = -A ( 1 ? 2) #LABB (3) 
A ( 1 i 5) = + A(1, 1 ) *L ABB ( 3) 
A ( 1 , 6) = -All, 1 ) *LABB (2) 
A (2? 4) = -A ( 2 7 2 ) #LABB (3) 
A ( 2 7 5) = +A ( 2 7 1) * L A B B ( 3 ) 
A (2? 6 ) = -A ( 2 7 1 ) #LABB (2) 
A ( 3 7 4 ) = -A ( 3 7 2) *LABB (3) 
A ( 3 , 5) = +A ( 3 7 1 ) *LABB (3) 
A ( 3 . 6) = -A ( 3 7 1) *LABB(2) 


+ A(1 ,3)*LABB(2) 
-Ad 7 3 ) *LABB ( 1 ) 
+ Ad ,2)*LABB(1> 
+ A ( 2 7 3 ) *LABB (2 > 

- A (2 » 3) *LABB ( 1 ) 
+ A (2 72) *LABB d > 
+ A (3 7 3 ) *LABB (2 ) 

- A(3?3)*LABB(1) 
+ A (3 7 2 ) #LABB ( 1 ) 


RECALLING THAT THE 3RD AND 4TH CELLS OF CA3 WERE CALCULATED AT THE 
START OF THE PROGRAM? CAD HAS NOW BEEN DETERMINED. TAKE THE 
TRANSPOSE OF C A ] FOR LATER USE. 

CALL TRANSP <A?37l27AT) 


SET UP THE 1ST AND 2ND CELLS OF CAD0T3. THE 3RD AND 4TH CELLS 
ARE ZERO. THE 1ST CELL IS [ ACBDOT 3 WHICH IS OBTAINED FROM 
THE FOLLOWING EQUATION: 

[ACBDOT] = -[ DMTC ][ ACB 3 + [ ACB ][ OMTB ] 


WHERE [ OMTC ] AND [ OMTB ] ARE TILDE MATRICES FOR THE 
PARACHUTE AND SRB ANGULAR VELOCITIES. 


ADOT (1 7 1 )=+OMC (3)*A(2, 1) -OMC (2)*A(3, 1) +OMB (3) *A( 1 ,2) -OMB (2)#A( 1 »3> 
ADOT d 7 2) =+DMC (3) *A (2 ? 2) -OMC (2) *A (3? 2) -OMB < 3 ) «- A ( 1 ? 1 ) +OMB ( 1 ) *A ( 1 , 3) 
ADOT d 7 3 ) = + OMC (3>*A(2,3) -OMC (2) *A (3. 3) +OMB (2) *A ( 1 , 1 ) -OMB ( 1 ) *A ( 1 » 2) 
ADOT (2, 1 ) =-OMC (3) *A< 1 . 1 ) +OMC (1 ) *A (3? 1) +DMB (3 ) *A ( 2 , 2) -OMB (2 ) *A ( 2 ? 3) 
ADOT (2,2)=-0MC(3)*A( 1 ,2) +OMC d ) *A ( 3 » 2) -OMB (3) *A ( 2 , 1 ) +OMB (1 ) *A (2, 3) 
ADOT (2 ,3)=-0MC (3>*A ( 1 , 3) +OMC d )*A<3,3) +OMB (2>*A (2, 1 > -OMB (1 )*A(2?2) 
ADOT (3 7 1 )=+OMC (2)*A< 1 , 1) -OMC (1 >*A(2, 1) +OMB (3)*A (3? 2) -OMB (2)*A(3,3) 

AD0T(3 72)=+0MC(2>*A(1.2)-0MCd>*A<2.2)-0MB(3>*A(3 7 1 > +OMB (1 )*A(3?3) 
ADOT (3 , 3) =+QMC (2 ) #A ( 1 , 3) -OMC (1 ) *A ( 2, 3) +OMB (2 ) *A (3 , 1 ) -OMB ( 1 ) *A ( 3 ? 2) 


THE 2ND CELL, -C ACBDOT 3C LTB 3 , IS OBTAINED BY USING THE 1ST CELL. 


ADOT d ,4) 
ADOT (1 ,5) 
ADOT (1,6) 
ADOT (2,4) 
ADOT (2,5) 
ADOT (2,6) 
ADOT (3,4) 
ADOT (3,5) 
ADOT (3,6) 


-ADOT ( 1 , 2) *LABB (3) 
+ADOT (1,1) *LABE (3) 
-ADOT ( 1,1) *LABB<2) 
-ADOT (2,2) #LABE (3) 
+ ADOT ( 2 ? 1) *LABE (3) 
-ADOT ( 2 , 1 ) #LABB (2) 
-ADOT ( 3 , 2) #LABB ( 3) 
+ADOT ( 3 , 1>*LABB(3> 
-ADOT ( 3 , 1 ) *LABB (2) 


+ ADOT ( 1,3) *LABB (2) 

- ADOT ( 1 , 3) *LAB.B ( 1) 
+ ADOT (1,2) *LABB ( 1 ) 
+ ADOT (2, 3) *LABB (2) 

- ADOT (2,3) *LABB ( 1 ) 
+ ADOT (2,2) *LABB ( 1 ) 
+ ADOT ( 3 , 3) *LABB ( 2 ) 

- ADOT (3,3) *LABB ( 1 ) 
+ ADOT (3,2) #LABB ( 1 ) 


*#■ # # # # 

ALL MATRICES NEEDED TO CALCULATE C FA 3 HAVE NOW BEEN 
SET UP. PERFORM THE REQUIRED OPERATIONS. 
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CALL MMUL (A,MINV,AMINV»3» 12,12) 

CALL MMUL ( AMI NV , AT , AM INVA , 3 , 1 2 , 3) 

CALL MAINV3 (AMI NVA, TERMA1 ) 

C 

CALL MATVEC ( AMI NV , F , AMI NVF , 3 , 1 2 ) 

CALL MATVEC (ADOT, OMEGA, ADOTOM ,3, 12) 

C 

C INSERT THE MINUS SIGN WHILE COMBINING CAMINVF3 AND CADOTOM3. 

DO 563 1=1,3 

563 TERMVl(I) = -AMINVF(I) - ADOTOM(I) 

C 

CALL MATVEC (TERMA 1 , TERMV1 , TERMV2, 3, 3) 

C 

C FINALLY, PERFORM THE STEP IN WHICH [ FA 3» A 12X1 MATRIX, IS 
C CALCULATED. 

CALL MATVEC ( AT , TERMV2 , FA , 12 , 3 ) 

C 

C THE CALCULATION OF C FA 3 COMPLETES THE DETERMINATION OF ALL THE 

C FORCES AND MOMENTS AT THE C.M.'S OF THE SRB AND PARACHUTE. 

C ADD C F 3 AND l FA 3 TO GET [ FTOTAL 3. 

DO 570 1=1,12 

570 FTOTAL ( I ) = F < I ) + FA < I > 

C 

C CALCULATE ALL ACCELERATIONS IN THE ’B’ AND T C r FRAMES. 

C l DOMEGA 3 = t MI NV ][ FTOTAL 3 

C 

CALL MATVEC (M INV, FTOTAL , DOMEGA , 12 , 1 2) 

C # * 

C SET UP THE 9X1 ACCELERATION MATRIX THAT IS TO BE INTEGRATED. 

DO 590 1=1,3 

D0M9 ( I ) = DOMEGA (I) 

D0M9U+3) = DOMEGA ( I +3 ) 

590 D0M9 ( I +6 ) = D0MEGA(I+9> 

C 

C END OF INTEGRATION LOOP. 

C 

c##****##*****#***'#**-#-##*## *#:#**##* 

IF (KUTTA .LT. 4) GO TO 400 
KUTT A = 0 
C 

C*# # # # # # 

C SET FLAGS TO CONTROL OUTPUT SECTION OF PROGRAM AND CUT-OFF. 

f* 

IF (TIME .GE. TPRINT) IPRFLG=1 
IF (TIME .GE. TMAX ) IC0FLG=1 
IF ( I PRFLG .EQ. 1) GO TO 701 
IF (ICOFLG .EQ. 1 ) GO TO 701 

C IF ALL FLAGS = 0, GO TO START OF INTEGRATION LOOP. 

GO TO 400 
701 CONTINUE 
C 

c*###*#* 
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CALCULATE OUTPUT PARAMETERS FOR PLOTTING. 

$ # # # 

CALC. THE 3-2-1 EULER ANGLES FOR THE ORIENTATIONS OF THE SRB AND 
PARACHUTE. 

ST2B = -ABIU.3) 

THETB = AS IN (ST2B) *CNV 

PS IB =ZT AN2 ( AB I ( 1 » 2) 7 ABI (1,1))*CNV 

PH IB =ZTAN2 ( AB I < 2 » 3 ) »ABI (3,3)) *CNV 

ST2C = - AC I ( 1 t 3 ) 

THETC = AS IN (ST2C) #CNV 
PSIC =ZTAN2 ( AC I ( 1 ,2) ,ACI (1 , 1 ) ) *CNV 
PH 1C =ZT.AN2 ( AC I (2,3) » ACK3 , 3) ) *CNV 
# # # 

CALC. FLIGHT PATH ANGLE AND AZIMUTH OF THE SRB AND PARACHUTE. 

GAMIB = ZSIN(~VIBI (3) 7 VI B) *CNV 
AZIIB = ZTAN2(VIBI (2) »VIBI (1 > ) *CNV 
GAMIC = ZSIN ( — VI Cl (3) » VIC) *CNV 
AZIIC = ZTAN2(VICI (2) ,VICI (i))*CNV 
# # # 

SET UP C A I V 3 TO CALCULATE THE SRB BANK ANGLE. 

SAZB = SIN <AZI IB/CNV) 

CAZE: = COS (AZI IB/CNV) 

SGMB = SIN (GAMIB/CNV) 

CGMB = COS (GAMIB/CNV) 

A I V ( 1 , 1 ) = CAZB*CGMB 

AI V ( 2 r 1) = SAZB*CGMB 

AI V ( 3 7 1 ) =-SGMB 

AIV(1,2) =-SAZB 

AI V ( 2 t 2) = CAZB 

AI V ( 3, 2) = 0.0 

AI V ( 1 7 3) = CAZB*SGMB 

AI V ( 2 t 3) = 5AZB*SGMB 

AI V ( 3 7 3) = CGMB 

CALL MMUL ( ABI , A I V , ABV 7 3 7 3 , 3 ) 

BANKS' = ZTAN£(ABV( 1 .2) 7-ABV( 1 73) )*CNV 

# # 

SET UP ERROR INDICATORS. 

CALL ORTH (ABI 7 ORT ABI > 

CALL ORTH (ACI ,ORTACI) 

CALL MATVEC ( A , DOMEGA 7 ADOMEG 7 3 , 1 2 ) 

# # # 

CALC. VECTOR MAGNITUDES; CONVERT ANG. VELOCITIES TO DEG/SEC. 

CALL VMAG ( V ABB 7 DUMM 7 V AB ) 

CALL VMAG (R IB I 7 DUMM 7 R IB ) 

CALL VMAG (RAE I 7 DUMM , RAB) 

CALL VMAG (RIC 1 7 DUMM 7 RIC) 

CALL VMAG ( GB 7 DUMM 7 GMAG) 

PDEGB = OME ( 1 ) *CNV 
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QDEGB = 0MB (2) *CNV 
RDEGB = 0MB ( 3 ) #CNV 
F'DEGC = 0MC(1)*CNV 
QDEG'C = 0MC(2)*CNV 
RDEGC = OMC ( 3) #CNV 
* * * 

SORT OUT THE PRINT CONTROL TIME LOGIC. 

IF ( I PRFLG .EG. 0) GO TO 740 
IPRFLG = 0 

TPRINT = TPRINT + DTPRT 
IF (T IME .LT. TNP 1 ) GO TO 720 
IF (TIME .LT. TNP2) GO TO 740 
720 CONTINUE 

* * PRINT THE STANDARD SET OF PARAMETERS. 

WRITE (6-853) 

WRITE (6,861 ) TI ME , ALTB , QBRB , V IB , ALPHAB, PHI AB, PDEGB , QDEGB , RDEGB, 
2GAMI B 7 AZ 1 1 B , RI BI <1> ,RIBI (2) , PS IB , THETB , PHI B 
WRITE (6,862) ALTC , QBRC , V IC , ALPHAC , PH I AC , PDEGC , QDEGC , RDEGC , 

2GAMI C, AZ 1 1 C, RICI (1),RICI(2), PS I C , THETC , PHI C 
WRITE (6,871) 

DO 730 1=1,12 

730 WRITE (6,803) FL ( I ) , FG ( I ) , FW ( I ) , FA ( I ) , FTOTAL < I ) , DOMEGA ( I ) , OMEGA ( I ) 
WRITE (6,87?) ORTABI , ORTACI 


* * BEGIN DIAGNOSTIC PRINT, IF DESIRED. 


733 


(6 


IF (IDIAG 
WRITE (6 
WRITE 
WRITE 
WRITE 
DO 733 
WRITE 


.LT. 

889) 


(6,808) 
1 = 1 ,3 
(6,812) 


1) GO TO 739 
VIB,VAB,VIC,RIB, RA'B , RI C 

RHO , QBRSB , QBRSC, CAMB , CNMB , CMMB , CAMC , CNMC , CMMC 
GMAG , MASSB , MASSC , BANKB 

(A(I ,J) , J=l, 12) 


DO 734 1=1,3 

734 WRITE (6,812) ( ADOT ( I , J) , J=1 , 1 2) 

WRITE (6,808) ADOMEG ( 1 ) , ADOTOM ( 1 ) , ADOMEG (2 ) , ADOTOM ( 2 ) , 
2AD0MEG (3) , ADOTOM (3) 

739 WRITE (6,802) 

740 CONTINUE 


END OF OUTPUT SECTION. 


*#**■*## 



IF ( I COFLG ,EQ. 0) GO TO 400 

- 


END OF 

RUN. 

’ 


STOP 


” 

300 

FORMAT 

( 1H1) 

■ 

SOI 

FORMAT 

( 1 6 A4 , I 1 , 12) 

: 

802 

FORMAT 

( 1H0 , 16A4, I 16,215) 

* 

305 

FORMAT 

(115, 15 , 2X » 4E 1 ! .8) 

- 

307 

FORMAT 

(7E1 1 .8) 


SOS 

FORMAT 

(1X,F1B.9,5F19.9»F18.9) 

: 

S09 

FORMAT 

(4X >9E1 4. 7) 


812 

FORMAT 

( IX , 1 IE 1 1 . 4 , E 1 0 . 3 ) 


820 

FORMAT 

( 20A4 ) 

- 

822 

FORMAT 

( 2X , 20A4) 

= 
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858 FORMAT < 3X , 4HT IME , 6X r 3HALT , 3X » 4HQBAR > 3X - 2HVI , 5X r 5HALPH A , 4X , 

B4HPHIA » 5X » IHPt 6X , 1 HQ , 6X * iHR , 5X , 5HGAMMA , 3X , 7HAZ IMUTH , 5X , 2HX 1 , 7X , 
C2HYI *8Xr 18H3-2-1 EULER ANGLES) 

861 FORMAT ( F8 . 3 , F9 . 1 » F6 . 1 , F7 . 2 7 3F8 . 2 r 2F7 . 2 7 2F9 . 3 , 2F9 . 1 , F9 .2 ,2F8 .2) 
362 FORMAT ( 8X , F9 . 1 , F6 . 1 , F7 . 2 , 3F8 . 2 , 2F7 . 2 , 2F9 . 3 r 2F9 . 1 , F9 . 2 , 2F8 . 2 ) 
871 FORMAT ( 10X r 2HFL» 17X »2HFG» 17X» 2HFW» 17X »2HFA»'17X » 6HFTQTAL » 13X » 
B6HD0MEGA » 1 2X » 5H0MEGA ) 

879 FORMAT (85X > 1 6HERR0R INDI GATORS , 2F 1 5 . 10) 

889 FORMAT < 6X , 1 1 HD I AGNOSTI CS ) 

END 
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